import os

from ament_index_python.packages import get_package_share_directory

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.conditions import IfCondition, UnlessCondition

import xacro


def generate_launch_description():
    camera_dir = os.path.join(get_package_share_directory('xarm_pubsub'), 'launch')

    
    node_robot_state_publisher = Node(
        package='xarm_pubsub',
        executable='master',
        name='right_master',#'master',
        remappings=[('/joint_states', '/right/joint_states')]
    )

    node_robot_subscriber = Node(
        package='xarm_pubsub',
        executable='puppet',
        name='right_puppet',
        remappings=[('/joint_states', '/right/joint_states')]
    )

    node_robot_subscriber_gui = Node(
        package='xarm_pubsub',
        executable='record_gui',
        remappings=[('/joint_states', '/right/joint_states')]
    )


    node_robot_image_sub = Node(
        package='xarm_pubsub',
        executable='image_sub',
        name='image_sub',
    )

    
    # Launch!
    return LaunchDescription([
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource([camera_dir, '/camera.launch.py']),
        ),
        #node_robot_state_publisher,
        #node_robot_image_pub,
        # node_robot_image_sub,
        node_robot_subscriber_gui,
        # node_robot_image_pub
        
    ])
